package novatel

import (
	"dgo/goutils/gnss"
	"dgo/goutils/utils"
	"encoding/binary"
	"fmt"
	"math"
)

type NovAtelRangeCmp struct {
	ObsN uint32
}

/*
*

	bufl : 24
*/
func (this *NovAtelRangeCmp) DecodeObs(week, tow uint32, buf []byte, transVars *gnss.GnssYObsTransVarCollect, obs *gnss.YObsSignal) error {
	trackval := gnss.U4FromBuf(buf)
	sys := trackval >> 16 & 7
	signtype := (trackval >> 21) & 0x1F
	plock := utils.GetBitValue0(uint64(trackval), 10)
	clock := utils.GetBitValue0(uint64(trackval), 12)

	/**
	Parity known flag 0 = Not known1 = Known
	*/
	//parity := utils.GetBitValue0(uint64(trackval), 11);

	/**
	Carrier phase measurement 0 = Half Cycle Not Added,	1 = Half Cycle Added
	*/
	halfc := utils.GetBitValue0(uint64(trackval), 28)

	obs.NaviSys = ToNaviSys(byte(sys))
	prn := buf[17]
	if obs.NaviSys == gnss.SYS_GLO {
		prn -= 37
	} else if obs.NaviSys == gnss.SYS_BD {
		prn -= 140
	} else if obs.NaviSys == gnss.SYS_SBS {
		prn -= 119
	}
	if prn <= 0 {
		return fmt.Errorf("错误的卫星号[%d]", prn)
	}
	obs.SatId = prn

	_, obs.SigId = RangecmpToYObsSignID(obs.NaviSys, byte(signtype))
	if obs.SigId == 0 {
		return fmt.Errorf("错误的频点[%s][%d]", gnss.GetNavSysCode(obs.NaviSys), signtype)
	}

	v0 := utils.IntFromU(uint64(gnss.U4FromBuf(buf[4:])), 28)
	obs.Doppler = float64(v0) / 256

	// psr=(U4(p+7)>>4)/128.0+U1(p+11)*2097152.0;
	// Pseudorange (PSR) 60-95
	//v0 := float64(int64(gnss.U4FromBuf(buf[7:]) >> 4)) / 128.0 + float64(buf[11]) * 2097152.0;
	psr := float64(int64(buf[11])<<28+int64(gnss.U4FromBuf(buf[7:])>>4)) / 128.0 // + float64(buf[11]) * 2097152.0;
	obs.PsV = psr

	// Accumulated Doppler Range (ADR) 96-127(32Bits)   scale:1/256   units:cycles
	/**
	adr=I4(p+12)/256.0;
	adr_rolls=(psr/wavelen+adr)/MAXVAL;
	adr=-adr+MAXVAL*floor(adr_rolls+(adr_rolls<=0?-0.5:0.5));
	*/
	adr := float64(gnss.U4FromBuf(buf[12:])) / 256.0
	wl := gnss.GetWaveLength(obs.NaviSys, obs.GetSignType(), obs.SatId)
	if wl == 0 {
		return fmt.Errorf("波长计算错误[%s][%d]", obs.GetPrnCode(), signtype)
	}
	if wl != 0 {
		adr_rolls := (psr/wl + adr) / MAXVAL
		var offsetv float64 = 0
		if adr_rolls <= 0 {
			offsetv = -0.5
		} else {
			offsetv = 0.5
		}
		adr = -adr + MAXVAL*math.Floor(adr_rolls+offsetv)
		obs.PhV = adr
	}

	// lockt=(U4(p+18)&0x1FFFFF)/32.0; /* lock time */ unit s
	// Lock Time 144-164(21) (maximum: 2,097,151)  scale: 1/32 unit:s
	lk := float64(gnss.U4FromBuf(buf[18:])&0x1FFFFF) / 32.0
	obs.LkT = int64(lk * 1000)

	// snr=((U2(p+20)&0x3FF)>>5)+20.0;
	// C/No (valid range: 20-51 dB-Hz) 165-169(5) (20 + n) dB-Hz
	obs.Cnr = float32(uint64(gnss.U2FromBuf(buf[20:])&0x3FF)>>5) + 20.0

	if plock == 0 {
		obs.PhV = 0
		obs.Doppler = 0
	}

	if clock == 0 {
		obs.PsV = 0
	}

	if transVars != nil {
		preVar := transVars.GetVar(obs.NaviSys, prn, obs.SigId)

		if preVar.Tow != 0 {
			t := int64(gnss.TowDiff(preVar.Tow, tow))
			if obs.LkT < 65535968 && obs.LkT-preVar.LockT+50 <= t {
				obs.LLI = 1
			} else {
				obs.LLI = 0
			}
		}

		if preVar.Half != halfc {
			obs.LLI |= 1
		}
		preVar.Half = halfc
		preVar.LockT = obs.LkT
		preVar.Tow = tow
		preVar.Week = week
	}
	return nil
}

func (this *NovAtelRangeCmp) DecodeBuf(week, tow uint32, buf []byte, transVars *gnss.GnssYObsTransVarCollect, onobs func(idx int, yobs *gnss.YObsSignal, err error) bool) error {
	this.ObsN = binary.LittleEndian.Uint32(buf)
	n := int(this.ObsN)
	l := 4 + n*24
	if len(buf) < l {
		return fmt.Errorf("oem4 rangecmpb length error: len=%d nobs=%d", len(buf), this.ObsN)
	}

	byteN := 4
	var obs gnss.YObsSignal
	for i := 0; i < n; i++ {
		// 初始化
		obs.Reset()
		err := this.DecodeObs(week, tow, buf[byteN:], transVars, &obs)
		if !onobs(i+1, &obs, err) {
			break
		}
		byteN += 24
	}

	return nil
}
